﻿//----------------------------------------------------------------------------
//  Copyright (C) 2004-2013 by EMGU. All rights reserved.       
//----------------------------------------------------------------------------

using System;
using System.Runtime.InteropServices;
using System.Windows.Forms;
using Emgu.CV;
using Emgu.CV.CvEnum;
using Emgu.CV.Structure;
using Emgu.CV.UI;
using System.Drawing;
using helloworld2;
using System.Collections.Generic;

// BACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUP
//BACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUP
//BACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUP
//BACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUP
// BACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUP
//BACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUP
//BACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUP
//BACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUP
// BACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUP
//BACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUP
//BACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUP
//BACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUPBACKUP


namespace helloworld2
{



    class Image
    {
        private bool foundPattern = false;
        private static int cityCounter = 0; 
        private static int roadCounter = 1;
        private static KeyValuePair<int, int> currentPosition;
        private KeyValuePair<int, int> yx;
        private List<KeyValuePair<int, int>> purples = new List<KeyValuePair<int, int>>();
        private static List<KeyValuePair<int, int>> visitedCities = new List<KeyValuePair<int, int>>();
        private static List<KeyValuePair<int, int>> visitedRoads = new List<KeyValuePair<int, int>>();
        
        private static List<KeyValuePair<int, int>> dontGoYellows = new List<KeyValuePair<int, int>>();
        private List<KeyValuePair<int, int>> newRoad = new List<KeyValuePair<int, int>>();
        private static string[] cities = { "Skagen", "Aalborg","Herning", "Aarhus","Vejle", "Koebenhavn", "Esbjerg", "Odense", "Aabenraa","Rødbyhavn"};
        private static Image<Bgr, Byte> img1 = new Image<Bgr, Byte>("C:/Users/Simonsk/Desktop/Electric car/trunk/helloworld2/helloworld2/danmark.tif");
        //C:/Users/Simonsk/Desktop/Electric car/trunk/helloworld2/helloworld2/test.tif   SIMONS
        //D:/UCN/3SemProjektwc/trunk/helloworld2/helloworld2/danmark3tif.tif   NICOLAIS
        private static bool reverse = false;

        private KeyValuePair<int, int> temp1;
        private KeyValuePair<int, int> temp2;

        RoadContainer rc = RoadContainer.Instance;
        int purpleCount = 0;
        int roadWidth = 0;
        bool checkPoint = false;
        bool isVisited;
        int seekX;
        int seekY;
        Bgr red = new Bgr(36, 28, 237);
        Bgr yellow = new Bgr(0, 242, 255);
        Bgr orange = new Bgr(0, 128, 255);
        Bgr purple = new Bgr(164, 73, 163);
        Bgr blue = new Bgr(204, 72, 63);
        Bgr test;

        City start;
        City finish;
        CityContainer cc = CityContainer.Instance;

        //SINGLETON BEGIN --------------------------------------------------------
        private static Image instance=null;
        
        private Image()
        {
        }
        public static Image Instance
        {
            get
            {
                if (instance==null)
                {
                    instance = new Image();
                }
                return instance;
            }
        }
        //SINGLETON END -----------------------------------------------------------

        
        public void loadMap()
        {
            // ITERATE THROUGH ALL PIXELS, AND PAINT MAP!!

            for (int i = 0; i < 2; i++)
            {
                for (int y = 0; y < img1.Height; y++) //Y-scale
                {
                    for (int x = 0; x < img1.Width; x++) //X-scale
                    {
                        currentPosition = new KeyValuePair<int, int>(y, x);

                        if (img1[y, x].Equals(red) && i == 0)
                        {
                            locateCities(y, x);
                        }

                        if (img1[y, x].Equals(purple) && i == 1)
                        {
                            isVisited = checkVisited(currentPosition, visitedRoads);
                            if (!isVisited)
                            {
                                locateRoad(y, x);
                            }
                        }
                    }
                }
            }
            addConnections();
        }

        public void locateCities(int y, int x)
        {
            isVisited = checkVisited(currentPosition, visitedCities);
            List<KeyValuePair<int, int>> fields = new List<KeyValuePair<int, int>>();
            if (!isVisited)
            {
                for (int vy = y; vy < y + 7; vy++)
                {
                    for (int vx = x; vx < x + 7; vx++)
                    {
                        yx = new KeyValuePair<int, int>(vy, vx);
                        visitedCities.Add(yx);
                        fields.Add(yx);
                        if (vy == y && vx == x)
                        {
                            img1[vy + 3, vx + 3] = orange;
                            City c = new City(cities[cityCounter], y + 3, x + 3,fields); //+3 for the center of red square
                            cityCounter++;
                            Console.WriteLine(fields);
                            fields.Clear();
                        }
                    }
                }
            }
            //Console.WriteLine("FINAL VISITED" + visited.Count);
            //Console.WriteLine("current " + currentPosition);
        }


        public void locateRoad(int y, int x)
        {


            findPattern(y, x);
            foundPattern = false;
            findYellows(y, x);
            
                
            //Console.WriteLine("New road counter " + newRoad.Count);

            Road r = new Road(roadCounter, newRoad);
            rc.addRoad(r);

            //addConnections();

            //for (int a = 0; a < newRoad.Count; a++)
            //{
            //    if (roadCounter % 2 != 0)
            //        test = orange;
            //    else
            //        test = blue;

            //    img1[newRoad[a].Key, newRoad[a].Value] = test;
            //}

            for (int b = 0; b < newRoad.Count; b++)
            {
                dontGoYellows.Add(newRoad[b]);               
            }

            newRoad.Clear();
            roadCounter++;
         
        }


        public Road getRoad(int id)
        {
            Road rr = rc.getRoad(id);
            Console.WriteLine("jijijijijij " + rr.getField().Count);
            Console.WriteLine("ajajajajajaja " + rr.getId());
            return rr;
            
        }



        public void findPattern(int y, int x)
        {
            roadWidth++;
            yx = new KeyValuePair<int, int>(y, x);
            visitedRoads.Add(yx);
            newRoad.Add(yx);
            if (!foundPattern && roadWidth < 3)
            {
                if (img1[y, x + 1].Equals(purple))
                {
                    //Vertical road
                    //yx = new KeyValuePair<int, int>(y, x + 1); kan ikke lige se denne linie er nødvendig...
                    findPattern(y, x + 1);

                }
                else if (img1[y + 1, x + 1].Equals(purple))
                {
                    //SKRÅ MOD SYDVEST
                    //yx = new KeyValuePair<int, int>(y + 1, x + 1);
                    findPattern(y + 1, x + 1);
                }
                else if (img1[y + 1, x].Equals(purple))
                {
                    //HORIZONTAL ROAD MOD ØST
                    //yx = new KeyValuePair<int, int>(y + 1, x);
                    findPattern(y + 1, x);
                }

                else if (img1[y + 1, x - 1].Equals(purple))
                {
                    //Skrå vej mod sydøst
                    //yx = new KeyValuePair<int, int>(y + 1, x - 1);
                    findPattern(y + 1, x - 1);
                }
            }
            if (roadWidth == 3)
            {
                foundPattern = true;
                roadWidth = 0;
            }
        }


        public void findYellows(int y, int x)
        {
            bool onYellow = true;
            bool foundEnd;
            int newXRight = x;
            int newXLeft = x;
            int i;          

            while (onYellow)
            {
                yx = new KeyValuePair<int, int>(y, x);

                if (img1[y, x].Equals(yellow) && !dontGoYellows.Contains(yx)) //Hvis den lander i gul
                {
                    newRoad.Add(yx);
                }
                else if (img1[y, x].Equals(purple)) //Hvis det er en purple                           
                {
                    if (newRoad.Contains(yx)) //Hvis det er en af de 3 første purples
                    {

                    }

                    else //Hvis det er en slut purple              Den skal stadig søge 
                    {
                        onYellow = false;    // BREAK STOD DER FØR
                    }
                }
                
                else //Hvis det er en hvid
                {
                    onYellow = false;
                }

                if (onYellow)
                {

                    foundEnd = false;
                    i = 1;
                    while (!foundEnd) //Søg til højre
                    {
                        yx = new KeyValuePair<int, int>(y, x + i);
                        if (img1[y, x + i].Equals(yellow) && !dontGoYellows.Contains(yx)) //Finder gul
                        {
                            newRoad.Add(yx);
                        }

                        else if (img1[y, x + i].Equals(purple) && newRoad.Contains(yx)) //Finder start lilla
                        {
                        }

                        else if (img1[y, x + i].Equals(purple) && !newRoad.Contains(yx)) //Finder slut lilla
                        {
                            //CheckSquare(y, x + i);
                            foundEnd = true;
                            newXRight = x + i;
                        }

                        else //Finder hvid
                        {
                            foundEnd = true;
                            newXRight = x + i - 1;
                        }
                        i++;
                    }

                    foundEnd = false;
                    i = 1;
                    while (!foundEnd) //Søg til venstre
                    {
                        yx = new KeyValuePair<int, int>(y, x - i);
                        if (img1[y, x - i].Equals(yellow) && !dontGoYellows.Contains(yx)) //Finder gul       
                        {
                            newRoad.Add(yx);
                        }

                        else if (img1[y, x - i].Equals(purple) && newRoad.Contains(yx)) //Finder start lilla
                        {
                        }

                        else if (img1[y, x - i].Equals(purple) && !newRoad.Contains(yx)) //Finder slut lilla
                        {
                            //CheckSquare(y, x - i);
                            foundEnd = true;
                            newXLeft = x - i;
                        }

                        else //Finder hvid
                        {
                            foundEnd = true;
                            newXLeft = x - i + 1;
                        }
                        i++;
                    }



                    int newX = ((newXRight - newXLeft) / 2) + newXLeft;
                    x = newX;

                    if (!img1[y + 1, newX].Equals(yellow) || !img1[y + 1, newX].Equals(purple))
                    {
                        for (int k = 1; k < (newXRight - newXLeft) / 2; k++)
                        {
                            if (img1[y + 1, newX - k].Equals(yellow))
                            {
                                x = newX - k;
                            }
                            else if (img1[y + 1, newX + k].Equals(yellow))
                            {
                                x = newX + k;
                            }
                        }
                    }
                    y++;
                }
            }
        }

        public void addConnections()
        {
            for (int i = 1; i < rc.getRoads().Count; i++)
            {
                Road rrr = rc.findRoad(i);
                Console.WriteLine("ggggggggg id " + rrr.getId() + "fieldcount " + rrr.getField().Count);
                


            }
        }

       

        public bool checkVisited(KeyValuePair<int, int> yx, List<KeyValuePair<int, int>> listToCheck)
        {
            bool isVisited = false;
            for (int i = 0; i < listToCheck.Count; i++)
            {
                if (yx.Equals(listToCheck[i]) && !isVisited)
                {
                    isVisited = true;
                }
            }          
            return isVisited;
        }

        public void checkSquare(int y, int x, Bgr color)
        {
            bool done = false;
            // Tager imod yx og en farve. Den finder den farve rundt om yx
            if (img1[y - 1, x - 1].Equals(color))
            {
                yx = new KeyValuePair<int, int>((y - 1), (x - 1));
                isVisited = checkVisited(yx, purples);
                if (!isVisited)
                {
                    purples.Add(yx);
                    done = true;
                }
            }

            if (img1[y - 1, x].Equals(color))
            {
                yx = new KeyValuePair<int, int>((y - 1), (x));
                isVisited = checkVisited(yx, purples);
                if (!isVisited)
                {
                    purples.Add(yx);
                    done = true;
                }
            }
            if (img1[y - 1, x + 1].Equals(color))
            {
                yx = new KeyValuePair<int, int>((y - 1), (x + 1));
                isVisited = checkVisited(yx, purples);
                if (!isVisited)
                {
                    purples.Add(yx);
                    done = true;
                }
            }
            if (img1[y, x - 1].Equals(color))
            {
                yx = new KeyValuePair<int, int>((y), (x - 1));
                isVisited = checkVisited(yx, purples);
                if (!isVisited)
                {
                    purples.Add(yx);
                    done = true;
                }
            }
            if (img1[y, x + 1].Equals(color))
            {
                yx = new KeyValuePair<int, int>((y), (x + 1));
                isVisited = checkVisited(yx, purples);
                if (!isVisited)
                {
                    purples.Add(yx);
                    done = true;
                }
            }
            if (img1[y + 1, x - 1].Equals(color))
            {
                yx = new KeyValuePair<int, int>((y + 1), (x - 1));
                isVisited = checkVisited(yx, purples);
                if (!isVisited)
                {
                    purples.Add(yx);
                    done = true;
                }
            }
            if (img1[y + 1, x].Equals(color))
            {
                yx = new KeyValuePair<int, int>((y + 1), (x));
                isVisited = checkVisited(yx, purples);
                if (!isVisited)
                {
                    purples.Add(yx);
                    done = true;
                }
            }
            if (img1[y + 1, x + 1].Equals(color))
            {
                yx = new KeyValuePair<int, int>((y + 1), (x + 1));
                isVisited = checkVisited(yx, purples);
                if (!isVisited)
                {
                    purples.Add(yx);
                    done = true;
                }
            }

            //Hvis den ikke finder noget vil den retunere (0,0) anden løsning måske??
            if (!done)
            {
                purples.Clear();
            }

        }

        private Object findObjectFromCoordinates(KeyValuePair<int, int> coordinates)
        {
            Object o = new Object();
            if (img1[coordinates.Key, coordinates.Value].Equals(red))
            {
            }
            else if (img1[coordinates.Key, coordinates.Value].Equals(yellow))
            {
                
            }
            else
            {
            }
            return o;
        }


        /* GAMLE DETECT HORIZONTAL ROADS

        public void locateRoads(int y, int x)
        {
            City cityEnd = null;
            City cityStart = null;
            List<KeyValuePair<int, int>> newRoad = new List<KeyValuePair<int, int>>();

            //improved horizontal
            bool onPurple = true;
            int k = 0;

            for (int i = 0; i < visitedRoads.Count; i++)
            {
                if (currentPosition.Equals(visitedRoads[i]) && !currentlyInVisited)
                {
                    currentlyInVisited = true;
                }
            }
            
            if (!currentlyInVisited)
            {
                while (onPurple)    //Count the purples, add them to visitedRoads, and get to know the road width
                {
                    if (img1[y + k, x].Equals(purple))               
                    {
                        Console.WriteLine(k);
                        if (k == 0)
                        {
                            temp1 = new KeyValuePair<int, int>(y + k, x);
                        }
                        else if (k == 1)
                        {
                            temp2 = new KeyValuePair<int, int>(y + k, x);
                        }
                        else if (k == 2)
                        {
                            visitedRoads.Add(temp1);
                            visitedRoads.Add(temp2);
                            newRoad.Add(temp1);
                            newRoad.Add(temp2);
                            yx = new KeyValuePair<int, int>(y + k, x);
                            visitedRoads.Add(yx);
                            newRoad.Add(yx);
                        }
                        else
                        {
                            yx = new KeyValuePair<int, int>(y + k, x);
                            visitedRoads.Add(yx);
                            newRoad.Add(yx);
                        }
                        k++;
                    }
                    else
                    {
                        Console.WriteLine("STOP");
                        onPurple = false;
                    }                 
                }

                //Find out what's to the left of this horizontal road
                if (img1[y, x - 1].Equals(red))
                {
                    if (img1[y + 1, x - 4].Equals(orange))
                    {
                        KeyValuePair<int, int> cityCenter = new KeyValuePair<int, int>(y + 1, x - 4);
                        cityStart = cc.findCityByCoordinates(cityCenter);
                    }
                   
                }

                //DO something at yellow pixels now within the k width                
                bool foundEnd;
                int l = 1;

                for (int p = 0; p < k; p++)
                {
                    Console.WriteLine("p = " + p + " k = " + k);

                    foundEnd = false;
                    while (!foundEnd)
                    {
                        if (img1[y + p, x + l].Equals(yellow))
                        {
                            yx = new KeyValuePair<int, int>(y + p, x + l);
                            newRoad.Add(yx);
                            img1[y + p, x + l] = blue;
                            l++;
                        }
                        else if (img1[y + p, x + l].Equals(purple))
                        {
                            foundEnd = true;
                            yx = new KeyValuePair<int, int>(y + p, x + l);
                            newRoad.Add(yx);
                            img1[y + p, x + l] = blue;

                            //If there's a city to the right, define cityEnd
                            if (p == k-1 && img1[y + p, x + l + 1].Equals(red))
                            {
                                if (img1[y + p - (k/2), x + l + 4].Equals(orange))
                                {
                                    KeyValuePair<int, int> cityCenter = new KeyValuePair<int, int>(y + p - 1, x + l + 4);
                                    cityEnd = cc.findCityByCoordinates(cityCenter);
                                    Road r = new Road(roadCounter, newRoad, cityStart, cityEnd);
                                    roadCounter++;
                                    Console.WriteLine(newRoad.Count);
                                }
                            }
                            //If purple is followed by a yellow, do something else.
                            if (p == k - 1 && img1[y + p, x + l + 1].Equals(yellow))
                            {
                                
                            }

                            l = 1;                            
                        }                  
                    }
                }
            }

       GAMLE DETECT HORIZONTAL ROADS SLUT */

        /*
        for (int i = 1; i < 7; i++) //Horizontal road
        {
            if (img1[currentPosition.Key + i, currentPosition.Value].Equals(purple))
            {
                if (i == 6)
                {

                    for (int a = 0; a < 7; a++)
                    {
                        yx = new KeyValuePair<int, int>(y+a, x);
                        visitedRoads.Add(yx);
                    }

                    if (img1[currentPosition.Key, currentPosition.Value - 1].Equals(red))
                    {
                        bool onRoad = true;
                        int rx = 1;
                        int ry = 0;

                        KeyValuePair<int, int> cityCenter = new KeyValuePair<int, int>(currentPosition.Key + 3, currentPosition.Value - 4);
                        cityStart = cc.findCityByCoordinates(cityCenter);

                        List<KeyValuePair<int, int>> newRoad = new List<KeyValuePair<int, int>>();

                        while (onRoad == true)
                        {
                            if (img1[currentPosition.Key + ry, currentPosition.Value + rx].Equals(yellow))
                            {
                                yx = new KeyValuePair<int, int>(currentPosition.Key + ry, currentPosition.Value + rx);
                                visitedRoads.Add(yx);
                                newRoad.Add(yx);
                                img1[currentPosition.Key + ry, currentPosition.Value + rx] = blue;
                            }

                            else if (img1[currentPosition.Key + ry, currentPosition.Value + rx].Equals(purple))
                            {
                                yx = new KeyValuePair<int, int>(currentPosition.Key + ry, currentPosition.Value + rx);
                                visitedRoads.Add(yx);
                                newRoad.Add(yx);
                                img1[currentPosition.Key + ry, currentPosition.Value + rx] = blue;
                                    
                                if (img1[currentPosition.Key + ry, currentPosition.Value + rx + 1].Equals(red) && !foundEnd)
                                {
                                    cityCenter = new KeyValuePair<int, int>(currentPosition.Key + ry + 3, currentPosition.Value + rx + 4);
                                    cityEnd = cc.findCityByCoordinates(cityCenter);
                                    foundEnd = true;
                                }
                                ry++;
                                rx = 0;

                            }
                            else
                            {
                                onRoad = false;
                                Road r = new Road(roadCounter, newRoad, cityStart, cityEnd); //Ved ikke helt om den overfører newRoad korrekt, MEN DET TROR JEG
                                Console.WriteLine("horizontal road startcity " + cityStart.getName());
                                Console.WriteLine("horizontal road endcity " + cityEnd.getName());
                                roadCounter++;
                            }
                            rx++;
                        }
                    }

                }
            }
        }
                
        for (int i = 1; i < 7; i++) // vertical road
        {
            if (img1[currentPosition.Key, currentPosition.Value + i].Equals(purple))
            {
                if (i == 6)
                {

                    for (int a = 0; a < 7; a++ )
                    {
                        yx = new KeyValuePair<int, int>(y, x+a);
                        visitedRoads.Add(yx);
                    }


                    if (img1[currentPosition.Key - 1, currentPosition.Value].Equals(red))
                    {
                        bool onRoad = true;
                        int rx = 0;
                        int ry = 1;

                        KeyValuePair<int, int> cityCenter = new KeyValuePair<int, int>(currentPosition.Key - 4, currentPosition.Value + 3);
                        cityStart = cc.findCityByCoordinates(cityCenter);

                        List<KeyValuePair<int, int>> newRoad = new List<KeyValuePair<int, int>>();

                        while (onRoad == true)
                        {
                            if (img1[currentPosition.Key + ry, currentPosition.Value + rx].Equals(yellow))
                            {
                                yx = new KeyValuePair<int, int>(currentPosition.Key + ry, currentPosition.Value + rx);
                                visitedRoads.Add(yx);
                                newRoad.Add(yx);
                                img1[currentPosition.Key + ry, currentPosition.Value + rx] = blue;
                            }

                            else if (img1[currentPosition.Key + ry, currentPosition.Value + rx].Equals(purple))
                            {
                                yx = new KeyValuePair<int, int>(currentPosition.Key + ry, currentPosition.Value + rx);
                                visitedRoads.Add(yx);
                                newRoad.Add(yx);
                                img1[currentPosition.Key + ry, currentPosition.Value + rx] = blue;


                                if (img1[currentPosition.Key + ry + 1, currentPosition.Value + rx].Equals(red) && !foundEnd)
                                {
                                    cityCenter = new KeyValuePair<int, int>(currentPosition.Key + ry + 4, currentPosition.Value + 3);
                                    cityEnd = cc.findCityByCoordinates(cityCenter);

                                    foundEnd = true;
                                }
                                rx++;
                                ry = 0;
                            }
                            else
                            {
                                onRoad = false;
                                Road r = new Road(roadCounter, newRoad, cityStart, cityEnd); //Ved ikke helt om den overfører newRoad korrekt, MEN DET TROR JEG                             
                                Console.WriteLine("vertical road startcity " + cityStart.getName());
                                Console.WriteLine("vertical road endcity " + cityEnd.getName());
                                roadCounter++;
                            }
                            ry++;
                        }
                    }
                }
            }
        }
        //Console.WriteLine("Visited count: " + visitedRoads.Count);
    }*/


        public Image<Bgr, Byte> getImage() {
            return img1;
        }

        public void findRoute(string startCity, string finishCity)
        {
            this.start = cc.findCity(startCity);
            this.finish = cc.findCity(finishCity);

            Console.WriteLine(start.getName() + start.getCoordinates());
            Console.WriteLine(finish.getName() + finish.getCoordinates());

            currentPosition = start.getCoordinates();            

            //calculateDirection();

            Console.WriteLine("seeky = " + seekY + " seekx = " + seekX);

            
            while (checkPoint == false)
            {
                //calculateDirection();
                //goToNext();
            }
        }

        /*
        public void calculateDirection()
        {
            //seekY
            if (currentPosition.Key < finish.getCoordinates().Key)
            {
                seekY = 1;
            }
            else if (currentPosition.Key == finish.getCoordinates().Key)
            {
                seekY = 2;
            }
            else
            {
                seekY = 0;
            }
            //seekX
            if (currentPosition.Value < finish.getCoordinates().Value)
            {
                seekX = 1;
            }
            else if (currentPosition.Value == finish.getCoordinates().Value)
            {
                seekX = 2;
            }
            else
            {
                seekX = 0;
            }
        }*/

        /*public void goToNext()
        {




            if (seekY == 0 && seekX == 2)//Go straight north
            {
                    if (img1[currentPosition.Key - 1, currentPosition.Value].Equals(red) || img1[currentPosition.Key - 1, currentPosition.Value].Equals(yellow))
                    {
                        currentPosition = new KeyValuePair<int, int>(currentPosition.Key - 1, currentPosition.Value);
                        img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                    }
                    else
                    {
                        reverse = true;
                    }
            }

            if (seekY == 0 && seekX == 1)//Go NorthEast
            {
                //head east
                if (img1[currentPosition.Key, currentPosition.Value + 1].Equals(red) || img1[currentPosition.Key, currentPosition.Value + 1].Equals(yellow))
                {
                    currentPosition = new KeyValuePair<int, int>(currentPosition.Key, currentPosition.Value + 1);
                    img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                }
                //head north
                else if (img1[currentPosition.Key - 1, currentPosition.Value].Equals(red) || img1[currentPosition.Key - 1, currentPosition.Value].Equals(yellow))
                {
                    currentPosition = new KeyValuePair<int, int>(currentPosition.Key - 1, currentPosition.Value);
                    img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                }
                else
                {
                    reverse = true;
                }
            }

            if (seekY == 2 && seekX == 1)//Go straight east
            {
                if (img1[currentPosition.Key, currentPosition.Value + 1].Equals(red) || img1[currentPosition.Key, currentPosition.Value + 1].Equals(yellow))
                {
                    currentPosition = new KeyValuePair<int, int>(currentPosition.Key, currentPosition.Value + 1);
                    img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                }
                else
                {
                    reverse = true;
                }
            }
              
            if (seekY == 1 && seekX == 1)//Go SouthEast
            {
                if (reverse == false)
                {
                    //head east
                    if (img1[currentPosition.Key, currentPosition.Value + 1].Equals(red) || img1[currentPosition.Key, currentPosition.Value + 1].Equals(yellow))
                    {
                        currentPosition = new KeyValuePair<int, int>(currentPosition.Key, currentPosition.Value + 1);
                        img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                    }
                    //head south
                    else if (img1[currentPosition.Key + 1, currentPosition.Value].Equals(red) || img1[currentPosition.Key + 1, currentPosition.Value].Equals(yellow))
                    {
                        currentPosition = new KeyValuePair<int, int>(currentPosition.Key + 1, currentPosition.Value);
                        img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                    }
                    else
                    {
                        reverse = true;
                    }
                }

                else //reverse true
                {
                    if (img1[currentPosition.Key + 1, currentPosition.Value].Equals(red) || img1[currentPosition.Key + 1, currentPosition.Value].Equals(yellow))
                    {
                        currentPosition = new KeyValuePair<int, int>(currentPosition.Key + 1, currentPosition.Value);
                        img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                    }
                    else if (img1[currentPosition.Key, currentPosition.Value + 1].Equals(red) || img1[currentPosition.Key, currentPosition.Value + 1].Equals(yellow))
                    {
                        currentPosition = new KeyValuePair<int, int>(currentPosition.Key, currentPosition.Value + 1);
                        img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                    }
                    else
                    {
                        reverse = false;
                    }
                }

            }

            if (seekY == 1 && seekX == 2)//Go straight south
            {
                if (img1[currentPosition.Key + 1, currentPosition.Value].Equals(red) || img1[currentPosition.Key + 1, currentPosition.Value].Equals(yellow))
                {
                    currentPosition = new KeyValuePair<int, int>(currentPosition.Key + 1, currentPosition.Value);
                    img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                }

                else if (img1[currentPosition.Key + 1, currentPosition.Value].Equals(orange))
                {
                    currentPosition = new KeyValuePair<int, int>(currentPosition.Key, currentPosition.Value - 1);
                }

                else
                {
                    reverse = true;
                    currentPosition = start.getCoordinates();
                }


                Console.WriteLine(reverse);
                Console.WriteLine(currentPosition + "    " + finish.getCoordinates());

            }

            if (seekY == 1 && seekX == 0)//Go SouthWest
            {
                
                if (reverse == false)
                {
                    //head west
                    if (img1[currentPosition.Key, currentPosition.Value - 1].Equals(red) || img1[currentPosition.Key, currentPosition.Value - 1].Equals(yellow))
                    {
                        currentPosition = new KeyValuePair<int, int>(currentPosition.Key, currentPosition.Value - 1);
                        img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                    }
                    //head south
                    else if (img1[currentPosition.Key + 1, currentPosition.Value].Equals(red) || img1[currentPosition.Key + 1, currentPosition.Value].Equals(yellow))
                    {
                        currentPosition = new KeyValuePair<int, int>(currentPosition.Key + 1, currentPosition.Value);
                        img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                    }

                    else
                    {
                        test++;
                        if (test > 5)
                            checkPoint = true;
                        reverse = true;
                        Console.WriteLine("HHEHEHE");
                        currentPosition = start.getCoordinates();
                    }

                }

                else // reverse true
                {
                    if (img1[currentPosition.Key + 1, currentPosition.Value].Equals(red) || img1[currentPosition.Key + 1, currentPosition.Value].Equals(yellow))
                    {
                        currentPosition = new KeyValuePair<int, int>(currentPosition.Key + 1, currentPosition.Value);
                        img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                    }

                    else if (img1[currentPosition.Key, currentPosition.Value - 1].Equals(red) || img1[currentPosition.Key, currentPosition.Value - 1].Equals(yellow))
                    {
                        currentPosition = new KeyValuePair<int, int>(currentPosition.Key, currentPosition.Value - 1);
                        img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                    }

                    else
                    {
                        reverse = false;
                    }

                }

                Console.WriteLine(reverse);
                Console.WriteLine(currentPosition + "    " + finish.getCoordinates());
                if (currentPosition.Key == finish.getCoordinates().Key && currentPosition.Value == finish.getCoordinates().Value)
                {
                    checkPoint = true;
                }

            }

            if (seekY == 2 && seekX == 0)//Go straight west
            {
                if (img1[currentPosition.Key, currentPosition.Value - 1].Equals(red) || img1[currentPosition.Key, currentPosition.Value - 1].Equals(yellow))
                {
                    currentPosition = new KeyValuePair<int, int>(currentPosition.Key, currentPosition.Value - 1);
                    img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                }
                else if(img1[currentPosition.Key, currentPosition.Value - 1].Equals(orange))
                {
                    currentPosition = new KeyValuePair<int, int>(currentPosition.Key, currentPosition.Value - 1);
                }
                else
                {
                    reverse = true;
                    currentPosition = start.getCoordinates();
                }

                


                Console.WriteLine(reverse);
                Console.WriteLine(currentPosition + "    " + finish.getCoordinates());

            }

            if (seekY == 0 && seekX == 0)//Go NorthWest
            {
                if (reverse == false)
                {
                    //head west
                    if (img1[currentPosition.Key, currentPosition.Value - 1].Equals(red) || img1[currentPosition.Key, currentPosition.Value - 1].Equals(yellow))
                    {
                        currentPosition = new KeyValuePair<int, int>(currentPosition.Key, currentPosition.Value - 1);
                        img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                    }
                    //head north
                    else if (img1[currentPosition.Key - 1, currentPosition.Value].Equals(red) || img1[currentPosition.Key - 1, currentPosition.Value].Equals(yellow))
                    {
                        currentPosition = new KeyValuePair<int, int>(currentPosition.Key - 1, currentPosition.Value);
                        img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                    }
                    else
                    {
                        test++;
                        if (test > 5)
                            checkPoint = true;
                        reverse = true;
                        Console.WriteLine("HHEHEHE");
                        currentPosition = start.getCoordinates();
                    }
                }

                else // reverse true
                {
                    if (img1[currentPosition.Key - 1, currentPosition.Value].Equals(red) || img1[currentPosition.Key - 1, currentPosition.Value].Equals(yellow))
                    {
                        currentPosition = new KeyValuePair<int, int>(currentPosition.Key - 1, currentPosition.Value);
                        img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                    }

                    else if (img1[currentPosition.Key, currentPosition.Value - 1].Equals(red) || img1[currentPosition.Key, currentPosition.Value - 1].Equals(yellow))
                    {
                        currentPosition = new KeyValuePair<int, int>(currentPosition.Key, currentPosition.Value - 1);
                        img1[currentPosition.Key, currentPosition.Value] = new Bgr(164, 73, 163);
                    }
                    else
                    {
                        reverse = false;
                    }
                }

            }


            if (currentPosition.Key == finish.getCoordinates().Key && currentPosition.Value == finish.getCoordinates().Value)
            {
                checkPoint = true;
            }

        } */ // gotonext
        


            //Image<Gray, float> img2 = img1.Convert<Gray, float>(); convert to grayscale
            //Matrix<float> src = new Matrix<float>(img2.Width, img2.Height); Create matrix
            //CvInvoke.cvConvert(src, img2);

            // Console.WriteLine(src[1,1]);
            // Console.WriteLine(img1[10, 10]);

            // Console.ReadLine();       
        
    }
}

/* TO DO: 
 * Sørg for at en vej der ender med et 90grader sving virker.
 */